
#include "../ip/i_nms.h"
#include "../algorithm/i_sort.h"
#include "i_harris.h"

namespace idl
{
  /*Starting at position (r,c) in image and moving forward 128 positions,
  compute dx*dx, dx*dy, dy*dy and store in three consecutive lines of dxx.
  Memory reads will occur in image one position around the line in all directions*/
  inline void i_diff_moment_filter_128(const unsigned char *const *image, int *dxx, int r, int c)
  {
    int p = 0, dx, dy, rp1 = r + 1, rm1 = r - 1, cp1 = c + 1, cm1 = c - 1;
    for(; p<128; ++p)
    {
      dx = (image[r][cm1 + p] - image[r][cp1 + p]) >> 1;
      dy = (image[rm1][c + p] - image[rp1][c + p]) >> 1;
      dxx[p      ] = dx*dx;
      dxx[p + 128] = dx*dy;
      dxx[p + 256] = dy*dy;
    }
  }

  /*Convolve five 128 positions long line triplets d0-d4 with the filter 14641.
  Put result in the line triplet g
  Assume that the result fits in integers, i.e. the input should be at most 28 bits to be absolutely sure*/
  inline void i_vertical_smooth_diff_moment_14641_128(const int *d0, const int *d1, const int *d2, const int *d3, const int *d4, int *g)
  {
    int c = 0,dd;
    for(;c<128;c++)
    {
      dd=d2[c];
      g[c]=d0[c]+(d1[c]<<2)+(dd<<2)+(dd<<1)+(d3[c]<<2)+d4[c];
      dd=d2[c+128];
      g[c+128]=d0[c+128]+(d1[c+128]<<2)+(dd<<2)+(dd<<1)+(d3[c+128]<<2)+d4[c+128];
      dd=d2[c+256];
      g[c+256]=d0[c+256]+(d1[c+256]<<2)+(dd<<2)+(dd<<1)+(d3[c+256]<<2)+d4[c+256];
    }
  }

  /*Convolve a 128 positions long line g with the filter 14641 and put result back in g,
  but shifted two steps (for inplace update), so output is of length 124.
  Assume that the result fits in integers, i.e. the input should be at most
  28 bits to be absolutely sure*/
  inline void i_horizontal_smooth_diff_moment_14641_128(int *g)
  {
    int c = 0;
    for (; c<124; c++)
    {
      g[c] = g[c] + (g[c + 1] << 2) + (g[c + 2] << 2) + (g[c + 2] << 1) + (g[c + 3] << 2) + g[c + 4];
    }
  }

  inline void i_harris_response_124(const int *gxx, const int *gxy, const int *gyy, float *s)
  {
    float dxx, dxy, dyy, d, t, k = 0.06f;
    int c = 0;
    for (; c<124; c++)
    {
      dxx = (float)gxx[c];
      dxy = (float)gxy[c];
      dyy = (float)gyy[c];
      d = dxx*dyy - dxy*dxy;
      t = dxx + dyy;
      s[c] = d - k*t*t;
    }
  }

  /*Compute harris strength s in a slice of image im starting at [top,left] and ending at (bottom,left+123]
  Store result in the same region of s. If compiling with SIMD, position left of s should always be 16 byte aligned.
  Memory reads will occur 3 pixels outside the area in all directions.
  Hence top and left should be at least 3, bottom should be at most height-3.
  If left is more than width-127 memory reads will occur beyond the end of each line.
  temp should be allocated to at least 128*18=2304 positions*/
  void i_harris_slice_124(const unsigned char *const *image, float **s, int left, int top, int bottom, int *wa)
  {
    int i, ip2mod5, lm2 = left - 2;
    int *dxx[5], *gxx, *gxy, *gyy;

    gxx = wa;
    gxy = gxx + 128;
    gyy = gxy + 128;

    dxx[0] = gyy + 128;           //for (i = 0; i<5; i++) IxIx[i] = gyy + (3 * i + 1) * 128;
    dxx[1] = gyy + 512;
    dxx[2] = gyy + 896;
    dxx[3] = gyy + 1280;
    dxx[4] = gyy + 1664;

    /*prefill the derivative moment buffer*/
    i_diff_moment_filter_128(image, dxx[(top-2) % 5], top-2, lm2);
    i_diff_moment_filter_128(image, dxx[(top-1) % 5], top-1, lm2);
    i_diff_moment_filter_128(image, dxx[(top  ) % 5], top,   lm2);
    i_diff_moment_filter_128(image, dxx[(top+1) % 5], top+1, lm2);
    
    for (i = top; i < bottom; i++)
    {
      ip2mod5 = (i + 2) % 5;
      /*Update derivative moment buffer*/
      i_diff_moment_filter_128(image, dxx[ip2mod5/*(i + 2) % 5*/], i + 2, lm2);
      /*Vertical smoothing of the three derivative moments*/
      i_vertical_smooth_diff_moment_14641_128(
        dxx[(i - 2) % 5], dxx[(i - 1) % 5], dxx[i % 5], dxx[(i + 1) % 5], dxx[ip2mod5/*(i + 2) % 5*/], gxx);
      /*Horizontal smoothing of the three derivative moments*/
      i_horizontal_smooth_diff_moment_14641_128(gxx);
      i_horizontal_smooth_diff_moment_14641_128(gxy);
      i_horizontal_smooth_diff_moment_14641_128(gyy);
      /*Compute Harris from the smoothed result*/
      i_harris_response_124(gxx, gxy, gyy, s[i] + left);
    }
  }

  /*Compute harris strength s of image im of size height x width. Output will be valid
  from [3,3] to (width-3,height-3). Memory reads and writes can occur 128 pixels beyond each line.
  If compiling with SSE2 all positions s[i][3] have to be 16 byte aligned.
  wa should be allocated to 128*18=2304 positions*/
  void i_harris_strength(const unsigned char *const*image, float **s, int width, int height, 
                         int left, int top, int right, int bottom,
                         int *wa)
  {
    assert(left >= 0 && top >= 0 && right <= width && bottom <= height);
    int col = left;
    for (; col < right; col += 124)
    {
      i_harris_slice_124(image, s, col, top, bottom, wa);
    }
  }

  /*Extract corners in region from [top,left] to (bottom,right) given 2D strength image with dimension (height x width).
  Extract at most max_nr of points. Saturate using approximately hor_blocks horizontally
  and ver_blocks vertically.
  Put results in coords  (stores x0, y0, x1, y1, ...).
  For safety, "coords" should be pre-allocated to the size of [2 x max_nr], and the 
  wa buffer should be allocated to at least the size of the image region width*height*/
  inline int i_harris_extract_corners(const float *const*strength,
                                      int *coords,
                                      int width, int height,
                                      int left, int top, int right, int bottom,
                                      int max_nr, int hor_blocks, int ver_blocks,
                                      float abs_strength_threshold,
                                      float *wa)
  {
    int i, j, bcount, count, w, h, hbs, vbs, anrb, max_per_block, abottom, aright, pos, maxpix;
    float *sb, pivot;
    int *xb, *yb;

    w = right - left;
    h = bottom - top;

    assert(w >= 2 && h >= 2);

    /*the block size is forced to be at least 2x2*/
    hbs = i_max(2, w / i_max(1, hor_blocks));
    vbs = i_max(2, h / i_max(1, ver_blocks));

    /*compute actual number of blocks*/
    anrb = ((w / hbs) + i_sign(w%hbs)) * ((h / vbs) + i_sign(h%vbs));

    /*compute the actual number of maximum allowed features per block*/
    max_per_block = max_nr / anrb;
    maxpix = (w*h) / 5; /*an hardcoded number - guarantees memory safe*/

    sb = wa + (maxpix << 1);
    xb = (int*)(sb + maxpix);
    yb = (int*)(xb + maxpix);

    count = 0;
    for (i = top; i < bottom; i += vbs)
    {
      abottom = i_min(bottom, i + vbs);
      for (j = left; j < right; j += hbs)
      {
        aright = i_min(right, j + hbs);
        bcount = i_dense_nms_squared_5x5_f(strength, xb, yb, sb, width, height, j/*left*/, i/*top*/, aright, abottom, maxpix, abs_strength_threshold);

        if (bcount > max_per_block)/*need to perform quick sort*/
        {
          pivot = i_quick_select<float>(sb, bcount, bcount - max_per_block - 1, wa);
          for (pos = 0; pos<bcount; pos++)
          {
            if (( sb[pos] > pivot ) && count < max_nr)
            {
              coords[(count << 1)    ] = xb[pos];
              coords[(count << 1) + 1] = yb[pos];
              count++;
            }
          }
        }
        else
        {
          for (pos = 0; (pos < bcount) && (count < max_nr); pos++)
          {
            coords[(count << 1)    ] = xb[pos];
            coords[(count << 1) + 1] = yb[pos];
            count++;
          }
        }
      }
    }
    return(count);
  }

  /*Extract corners in region from [top,left] to (bottom,right) given 2D strength image with dimension (height x width).
  Extract at most max_nr of points. Saturate using approximately hor_blocks horizontally
  and ver_blocks vertically.
  Put results in coords  (stores x0, y0, x1, y1, ...).
  For safety, "coords" should be pre-allocated to the size of [2 x max_nr], and the
  wa buffer should be allocated to at least the size of the image region width*height*/
  inline int i_harris_extract_corners(const float *const*strength,
                                      unsigned short *coords,
                                      int width, int height,
                                      int left, int top, int right, int bottom,
                                      int max_nr, int hor_blocks, int ver_blocks,
                                      float abs_strength_threshold,
                                      float *wa)
  {
    int i, j, bcount, count, w, h, hbs, vbs, anrb, max_per_block, abottom, aright, pos, maxpix;
    float *sb, pivot;
    int *xb, *yb;

    w = right - left;
    h = bottom - top;

    assert(w >= 2 && h >= 2);

    /*the block size is forced to be at least 2x2*/
    hbs = i_max(2, w / i_max(1, hor_blocks));
    vbs = i_max(2, h / i_max(1, ver_blocks));

    /*compute actual number of blocks*/
    anrb = ((w / hbs) + i_sign(w%hbs)) * ((h / vbs) + i_sign(h%vbs));

    /*compute the actual number of maximum allowed features per block*/
    max_per_block = max_nr / anrb;
    maxpix = (w*h) / 5; /*an hardcoded number - guarantees memory safe*/

    sb = wa + (maxpix << 1);
    xb = (int*)(sb + maxpix);
    yb = (int*)(xb + maxpix);

    count = 0;
    for (i = top; i < bottom; i += vbs)
    {
      abottom = i_min(bottom, i + vbs);
      for (j = left; j < right; j += hbs)
      {
        aright = i_min(right, j + hbs);
        bcount = i_dense_nms_squared_5x5_f(strength, xb, yb, sb, width, height, j/*left*/, i/*top*/, aright, abottom, maxpix, abs_strength_threshold);

        if (bcount > max_per_block)/*need to perform quick sort*/
        {
          pivot = i_quick_select<float>(sb, bcount, bcount - max_per_block - 1, wa);
          for (pos = 0; pos<bcount; pos++)
          {
            if ((sb[pos] > pivot) && count < max_nr)
            {
              coords[(count << 1)] = (unsigned short)xb[pos];
              coords[(count << 1) + 1] = (unsigned short)yb[pos];
              count++;
            }
          }
        }
        else
        {
          for (pos = 0; (pos < bcount) && (count < max_nr); pos++)
          {
            coords[(count << 1)] = (unsigned short)xb[pos];
            coords[(count << 1) + 1] = (unsigned short)yb[pos];
            count++;
          }
        }
      }
    }
    return(count);
  }

  FastHarrisDetector::FastHarrisDetector() : _width(0), _height(0), _max_nr_fea(0), _nr_hor_blocks(1), _nr_ver_blocks(1)
  {
    _strength = NULL;
    _tempf = NULL;
    _tempi = NULL;
    _mem = NULL;
  }

  FastHarrisDetector::FastHarrisDetector(int width, int height) : _width(width), _height(height)
  {
    _nr_hor_blocks = I_DEFAULT_NR_BLOCKS_HORIZONTAL;
    _nr_ver_blocks = I_DEFAULT_NR_BLOCKS_VERTICAL;
    _max_nr_fea = i_min(I_DEFAULT_NR_INTEREST_POINTS_PER_IMAGE, (_width*_height) / 9);
    bool initialized = allocMemory(_height, _width);
    /*default ROI*/
    _roi[0] = 3;
    _roi[1] = 3;
    _roi[2] = _width - 4;
    _roi[3] = _height - 4;
    assert(initialized);
  }

  FastHarrisDetector::~FastHarrisDetector()
  {
    cleanup();
  }

  bool FastHarrisDetector::init(int width, int height)
  {
    assert(width > 0 && height > 0);
    bool result = true;
    if (_width != width || _height != height)
    {
      cleanup();
      result = allocMemory(height, width);
      if (result)
      {
        _nr_hor_blocks = I_DEFAULT_NR_BLOCKS_HORIZONTAL;
        _nr_ver_blocks = I_DEFAULT_NR_BLOCKS_VERTICAL;
        _max_nr_fea = i_min(I_DEFAULT_NR_INTEREST_POINTS_PER_IMAGE, (_width*_height) / 9);
        
        /*default ROI*/
        _roi[0] = 3;
        _roi[1] = 3;
        _roi[2] = _width - 4;
        _roi[3] = _height - 4;
      }
    }
    return result;
  }

  bool FastHarrisDetector::allocMemory(int m/*# of rows*/, int n/*# of columns*/)
  {
    assert(m > 0 && n > 0);
    _width = n;
    _height = m;
    _tempf = i_alloc<float>(m*n);
    _tempi = i_alloc<int>((18 << 7)); /*128*18*/
    /*compute the size of padded row*/
    int na = n - (n % 16) + 16 + 128;
    /*Compute total allocation size for strength image*/
    int ts = m*na + 16 + 128;
    /*allocate strength image memory bulk*/
    _mem = i_alloc<float>(ts);
    /*fill with zeroes*/
    i_zero(_mem, ts);
    /*get a 16-byte aligned pointer*/
    float *ap = i_align_pointer(_mem + 3, 16/*vv_harris uses 64 for this input argument, no idea why*/) - 3;
    /*allocate row pointers*/
    _strength = i_alloc<float*>(m);
    /*Set the row pointers*/
    i_make_reference(ap, _strength, m, na);
    return ((_strength != NULL) && (_tempf != NULL) && (_tempi != NULL));
  }

  void FastHarrisDetector::cleanup()
  {
    _width = _height = 0;
    _max_nr_fea = 0;
    _nr_hor_blocks = _nr_ver_blocks = 1;
    i_zero4(_roi);
    i_free(_tempf);
    i_free(_tempi);
    i_free(_strength);
    i_free(_mem);
  }

  int FastHarrisDetector::getMaxNrFeatures() const
  {
    return _max_nr_fea;
  }

  int FastHarrisDetector::setMaxNrFeatures(int nr)
  {
    _max_nr_fea = i_min(nr, (_width*_height) / 9); /*given the fact that we are using dense 5x5 nms, the every 9 pixel can have at most one feature*/
    return _max_nr_fea;
  }

  void FastHarrisDetector::setRoi(int left, int top, int right, int bottom)
  {
    //assert(left <= right && top <= bottom);
    if (left > right){ right = left + 1; }
    if (top > bottom){ bottom =  top + 1; }
    _roi[0] = i_max(3, left);
    _roi[1] = i_max(3, top);
    _roi[2] = i_min(_width - 4, right);
    _roi[3] = i_min(_height - 4, bottom);
  }

  int FastHarrisDetector::setNrBlocks(int nrbh, int nrbv)
  {
    _nr_hor_blocks = i_max(1, nrbh); 
    _nr_ver_blocks = i_max(1, nrbv); 
    return (_nr_hor_blocks*_nr_ver_blocks);
  }

  int FastHarrisDetector::detect(const unsigned char *const *image, int* coords, float abs_threshold)
  {
    int count = 0;
    int left = i_max(5, _roi[0]);
    int top = i_max(5, _roi[1]);
    int right = i_min(_width - 5, _roi[2] + 1);
    int bottom = i_min(_height - 5, _roi[3] + 1);
    i_harris_strength(image, _strength, _width, _height, _roi[0], _roi[1], (_roi[2]+1), (_roi[3]+1), _tempi);
    count = i_harris_extract_corners(_strength, coords, _width, _height, left, top, right, bottom, _max_nr_fea,
                                     _nr_hor_blocks, _nr_ver_blocks, abs_threshold, _tempf);
    return(count);
  }

  int FastHarrisDetector::detect(const unsigned char *const *image, unsigned short* coords, float abs_threshold)
  {
    int count = 0;
    int left = i_max(5, _roi[0]);
    int top = i_max(5, _roi[1]);
    int right = i_min(_width - 5, _roi[2] + 1);
    int bottom = i_min(_height - 5, _roi[3] + 1);
    i_harris_strength(image, _strength, _width, _height, _roi[0], _roi[1], (_roi[2] + 1), (_roi[3] + 1), _tempi);
    count = i_harris_extract_corners(_strength, coords, _width, _height, left, top, right, bottom, _max_nr_fea,
      _nr_hor_blocks, _nr_ver_blocks, abs_threshold, _tempf);
    return(count);
  }
}/* namespace idl */